package various;


import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.ColorSensor;

public class Ruota {

	private static int angle;
	private static int cicli;

	public static void main(String[] args) throws Exception {

		angle = (230 / 3) * 2;
		cicli = 18;

		Colori.initColori();
		ColorSensor cs = new ColorSensor(SensorPort.S1);

		calibra(cs);
		Thread.sleep(5000);

		String a = "";

		for (int i = 0; i < cicli; i++) {
			Motor.A.regulateSpeed(true);
			LCD.clear();
			LCD.drawString(Colori.getColore(cs.getColorNumber()), 0, 1);
			a += "|" + cs.getColorNumber();
			Thread.sleep(2000);
			Motor.A.rotate(angle);
			Motor.A.stop();
		}

		Thread.sleep(2000);
		LCD.clear();
		LCD.drawString(a, 0, 1);
		Thread.sleep(2000);
		riavvolgi(new UltrasonicSensor(SensorPort.S2));

	}

	private static void riavvolgi(UltrasonicSensor us) throws Exception {
		LCD.clear();
		LCD.drawString("Rewinding...", 0, 1);
		Thread.sleep(1000);
		while (us.getDistance() >= 5) {
			Motor.A.setSpeed(500);
			Motor.A.backward();
			LCD.clear();
			LCD.drawString("cm: " + us.getDistance(), 0, 2);
		}

	}

	public static void calibra(ColorSensor cs) throws Exception {
		Motor.A.regulateSpeed(true);
		int c = 1;
		int d = 1;
		while (c == 1 && d == 1) {
			LCD.clear();
			LCD.drawString("Avvio calibrazione bianco...", 0, 2);
			c = cs.initWhiteBalance();
			LCD.drawString("Calibrazione bianco eseguita", 0, 3);
			LCD.drawInt(c, 0, 3);
			Thread.sleep(2000);

			Motor.A.rotate(angle);
			LCD.clear();
			LCD.drawString("Avvio calibrazione nero...", 0, 2);
			d = cs.initBlackLevel();
			LCD.clear();
			LCD.drawString("Calibrazione nero eseguita", 0, 3);
			LCD.drawInt(d, 0, 6);
			Thread.sleep(2000);
		}
		LCD.clear();
		Motor.A.regulateSpeed(true);
		Motor.A.rotate(-angle);
		Thread.sleep(2000);

	}

}
